#ifndef RECONSTRUCTION_HELPERS_HPP
#define RECONSTRUCTION_HELPERS_HPP

#include "typeHelpers.hpp"

namespace hms::reconstruction
{

#ifndef NDEBUG
#define ASSERT_MATCHING_SIZES(...) hms::reconstruction::assertMatchingSizes(__VA_ARGS__)
#else
#define ASSERT_MATCHING_SIZES(...)
#endif

template<typename A, typename B>
void assertMatchingSizes(
	const Pair<A>& dCellLR,
	const Pair<A>& zCellLR,
	const Pair<B>& vCellLR
){
	static_assert(
		A::RowsAtCompileTime == 1 &&
		B::RowsAtCompileTime == 2
	);
	auto n { dCellLR[0].size() };
	assert((
		   ( n == dCellLR[1].size() )
		&& ( n == zCellLR[0].size() )
		&& ( n == zCellLR[1].size() )
		&& ( n == vCellLR[0].cols() )
		&& ( n == vCellLR[1].cols() )
	));
}

template<typename T>
struct PairHelper {};

template<typename T>
struct PairHelper<Pair<T>> {
	using Type = T;
};

template<typename T>
using PairType = typename PairHelper<T>::Type;

template<typename T>
constexpr bool isPair(){
	return std::is_same_v<
		remove_qualifiers<T>,
		Pair<PairType<remove_qualifiers<T>>>
	>;
}

template<Index nRows, typename Buf>
Pair<ArrayNXsMap<nRows>> mapPair(
	Eigen::DenseBase<Buf>& buf, Index nCols
){
	assert(buf.size() >= 2*nRows*nCols );
	return {{
		{ buf.derived().data(), nRows, nCols },
		{ buf.derived().data() + nRows*nCols, nRows, nCols }
	}};
}

template<typename A, typename B>
void setLR( Eigen::DenseBase<A>& lr, const B& val ){
	lr.derived().colwise() = val;
}

template<typename A, typename B>
void setLR( Pair<A>& lr, const B& val ){
	lr[0] = val;
	lr[1] = lr[0]; // if B is an expression type, no need to evaluate it twice
}

template<typename T>
auto meanLR( const Pair<T>& lr ){
	return 0.5 * ( lr[0] + lr[1] );
}

template<typename T>
auto meanLR( const Eigen::DenseBase<T>& lr ){
	return lr.derived().rowwise().mean();
}

template<typename T>
auto side(const Eigen::DenseBase<T>& lr, Index i){
	static_assert( T::ColsAtCompileTime == 2 );
	return lr.col(i);
}

template<typename T>
auto side(Eigen::DenseBase<T>& lr, Index i){
	static_assert( T::ColsAtCompileTime == 2 );
	return lr.col(i);
}

template<typename T>
const T& side(const Pair<T>& lr, Index i){
	return lr[i];
}

template<typename T>
T& side(Pair<T>& lr, Index i){
	return lr[i];
}

template<typename LR>
auto left( LR&& lr ){
	return side( std::forward<LR>(lr), 0 );
}

template<typename LR>
auto right( LR&& lr ){
	return side( std::forward<LR>(lr), 1 );
}

template<typename MapType, typename T>
using MapOrRef = std::conditional_t
	< isDenseType<remove_qualifiers<T>>(), std::add_lvalue_reference_t<T>, MapType >;

template<typename MapType, typename LR>
MapOrRef<MapType, LR>
mapOrRefLR( LR&& lr ){
	using LR_unq = remove_qualifiers<LR>;
	if constexpr ( isDenseType<LR_unq>() ){
		return lr;
	} else if constexpr ( std::is_same_v< LR_unq, Pair<PairType<LR_unq>> > ){
		assert( lr[1].data() == lr[0].data() + lr[0].size() );
		assert(
			lr[0].rows() == lr[1].rows() &&
			lr[0].cols() == lr[1].cols()
		);
		return { lr[0].data(), lr[0].rows(), lr[0].cols()*2 };
	} else {
		assert(false);
	}
}

template<typename LR>
constexpr Index nRowsLR(){
	using LR_unq = remove_qualifiers<LR>;
	if constexpr ( isDenseType<LR_unq>() ){
		return LR_unq::RowsAtCompileTime;
	} else {
		static_assert( std::is_same_v< LR_unq, Pair<PairType<LR_unq>> > );
		return PairType<LR_unq>::RowsAtCompileTime;
	}
}

} // namespace hms::reconstruction


#endif